#include <ros/ros.h>
#include <iostream>
#include <cmath>
#include <tf/transform_datatypes.h>
#include <sstream>

#include "prometheus_msgs/DroneState.h"
#include "prometheus_msgs/ControlCommand.h"
#include "printf_utils.h"
#include "mission_utils.h"
// 自定义的msg，用来画曲线
#include "visual_servo_landing/MyPlot.h"

using namespace std;
// using namespace Eigen;
#define NODE_NAME "my_landing"

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
bool g_use_pad_height; // 是否使用降落板绝对高度
float g_pad_height;
int g_uav_id;
std_msgs::Bool vision_switch;
float max_height; // 起始降落位置
float g_camera_offset[3];
prometheus_msgs::ControlCommand g_command_now;
// prometheus_msgs::UAVControlState g_uavcontrol_state; // 遥控器状态
std::string curr_mode;

// 消息打印
std::stringstream ss;

visual_servo_landing::MyPlot plot_msg;   // 作图的自定义消息

//---------------------------------------Drone---------------------------------------------
prometheus_msgs::DroneState g_UAVState; // 无人机状态
Eigen::Matrix3f g_R_Body_to_ENU;      // 无人机机体系至惯性系旋转矩阵
Eigen::Matrix3f R_c2v;      // 无人机相机坐标系到虚拟相机坐标系的旋转矩阵
//---------------------------------------Vision---------------------------------------------
nav_msgs::Odometry g_GroundTruth; // 降落板真实位置（仿真中由Gazebo插件提供）
// include/mission_utils.h 中定义的视觉检测结构体
Detection_result g_landpad_det;   // 检测结果
//---------------------------------------Track---------------------------------------------
// float g_kp_land[3]; // 控制参数，控制无人机的速度

// ----------------------------image & performance function------------------------------
// 图像特征
float Zd = -0.2;
// float ad = 4.43e-1;
float ad;

// Eigen::Vector3f s = Eigen::Vector3f::Zero();
Eigen::Vector3f sd = {0, 0, 1};

Eigen::Vector3f ks = {2.1, 2.1, 2.6};
Eigen::Matrix3f K_s = ks.asDiagonal();
Eigen::Vector3f ups = {10, 10, 8};
Eigen::Matrix3f Ups = ups.asDiagonal();
Eigen::Vector3f k_m = {10, 10, 1};
Eigen::Matrix3f K_m = k_m.asDiagonal();

Eigen::Matrix3f rho_s = Eigen::Matrix3f::Zero();
Eigen::Vector3f rho_st0 = {15, 15, 40};
Eigen::Vector3f rho_stinf = {.8, .8, .8};
Eigen::Vector3f ls = {0.5, 0.5, 0.5};

Eigen::Matrix3f d_rho_s = Eigen::Matrix3f::Zero();
Eigen::Vector3f e_s = Eigen::Vector3f::Zero();
Eigen::Vector3f xi_s = Eigen::Vector3f::Zero();
Eigen::Vector3f ep_s = Eigen::Vector3f::Zero();
Eigen::Matrix3f D_s = Eigen::Matrix3f::Zero();
Eigen::Matrix3f L_s = Eigen::Matrix3f::Zero();

Eigen::Vector3f hat_vm = Eigen::Vector3f::Zero();
Eigen::Vector3f v_r = Eigen::Vector3f::Zero();

// 相机内参
Eigen::Matrix3f camera_matrix = Eigen::Matrix3f::Zero();
float fx = 816.80845;
float fy = 818.41599;
float cx = 311.44342;
float cy = 214.47836;

// 四种状态机
enum EXEC_STATE
{
    WAITING,
    TRACKING_STAGE1,
    TRACKING_STAGE2,
    LOST,
    LANDING,
};
EXEC_STATE exec_state;

bool tracking_stage_change_flag = false;

float g_distance_to_pad;
float g_arm_height_to_ground;
float g_arm_distance_to_pad;
//---------------------------------------Output---------------------------------------------
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void landpadDetCb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
    g_landpad_det.object_name = "landpad";
    g_landpad_det.Detection_info = *msg;
    // 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    // 相机安装误差 在mission_utils.h中设置场
    // 相机坐标向下，机体系坐标是东北天的设置，x, y轴交换并反向，z轴反向且偏差为负
    // 相机系 -> 机体系
    g_landpad_det.pos_body_frame[0] = -g_landpad_det.Detection_info.position[1] + g_camera_offset[0];
    g_landpad_det.pos_body_frame[1] = -g_landpad_det.Detection_info.position[0] + g_camera_offset[1];
    g_landpad_det.pos_body_frame[2] = -g_landpad_det.Detection_info.position[2] + g_camera_offset[2];

    // 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
    g_landpad_det.pos_body_enu_frame = g_R_Body_to_ENU * g_landpad_det.pos_body_frame;

    if (g_use_pad_height)   // 设置是true
    {
        //若已知降落板高度，则无需使用深度信息。
        g_landpad_det.pos_body_enu_frame[2] = g_pad_height - g_UAVState.position[2];
    }

    // 机体惯性系 -> 惯性系，目标在惯性系的位置 = 飞机位置 + 目标在机体惯性系的位置
    g_landpad_det.pos_enu_frame[0] = g_UAVState.position[0] + g_landpad_det.pos_body_enu_frame[0];
    g_landpad_det.pos_enu_frame[1] = g_UAVState.position[1] + g_landpad_det.pos_body_enu_frame[1];
    g_landpad_det.pos_enu_frame[2] = g_UAVState.position[2] + g_landpad_det.pos_body_enu_frame[2];
    // 此降落方案不考虑偏航角
    g_landpad_det.att_enu_frame[2] = 0.0;

    // -------------------------------------------------------------------------------------
    // 目标获得和丢失数积累
    if (g_landpad_det.Detection_info.detected)
    {
        g_landpad_det.num_regain++;
        g_landpad_det.num_lost = 0;
    }
    else
    {
        g_landpad_det.num_regain = 0;
        g_landpad_det.num_lost++;
    }

    // 目标丢失，标志位变成false
    if (g_landpad_det.num_lost > VISION_THRES)
    {
        g_landpad_det.is_detected = false;
    }

    // 目标得到，标志变成true
    if (g_landpad_det.num_regain > VISION_THRES)
    {
        g_landpad_det.is_detected = true;
    }
}

void droneStateCb(const prometheus_msgs::DroneState::ConstPtr &msg)
{
    g_UAVState = *msg;

    curr_mode = msg->mode;

    g_R_Body_to_ENU = get_rotation_matrix(g_UAVState.attitude[0], g_UAVState.attitude[1], g_UAVState.attitude[2]);

    Eigen::Matrix3f R_c2B = Eigen::Matrix3f::Zero();
    R_c2B(0,1) = -1;
    R_c2B(1,0) = -1;
    R_c2B(2,2) = -1;

    R_c2v = g_R_Body_to_ENU * R_c2B;
}

inline void readParams(const ros::NodeHandle &nh)
{
    nh.param<int>("uav_id", g_uav_id, 1);
    //强制上锁高度
    nh.param<float>("arm_height_to_ground", g_arm_height_to_ground, 0.4);
    //强制上锁距离
    nh.param<float>("arm_distance_to_pad", g_arm_distance_to_pad, 0.3);
    // 是否使用降落板绝对高度
    nh.param<bool>("use_pad_height", g_use_pad_height, false);
    nh.param<float>("pad_height", g_pad_height, 0.01);

    // 目标丢失时，最大到飞行高度，如果高于这个值判定为任务失败
    nh.param<float>("max_height", max_height, 3.0);

    // 相机安装偏移,规定为:相机在机体系(质心原点)的位置
    nh.param<float>("camera_offset_x", g_camera_offset[0], 0.0);
    nh.param<float>("camera_offset_y", g_camera_offset[1], 0.0);
    nh.param<float>("camera_offset_z", g_camera_offset[2], 0.0);
}

inline void topicSub(ros::NodeHandle &nh)
{
    //【订阅】降落板与无人机的相对位置及相对偏航角  单位：米   单位：弧度
    //  方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    //  标志位：   detected 用作标志位 true代表识别到目标 false代表丢失目标
    static ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpadDetCb);

    // 无人机状态
    static ros::Subscriber drone_state_sub = nh.subscribe<prometheus_msgs::DroneState>("/prometheus/drone_state", 10, droneStateCb);

    // 地面真值，此信息仅做比较使用 不强制要求提供
    static ros::Subscriber groundtruth_sub = nh.subscribe<nav_msgs::Odometry>("/ground_truth/landing_pad", 10, [&](const nav_msgs::Odometry::ConstPtr &msg)
                                                                              { g_GroundTruth = *msg; });

    // 订阅遥控器状态
    // static ros::Subscriber uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>("/uav" + std::to_string(g_uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void
                                                                                                //   { g_uavcontrol_state = *msg; });
}

static ros::Publisher g_vision_switch_pub, g_command_pub, plot_pub;

inline void topicAdv(ros::NodeHandle &nh)
{
    // 【发布】 视觉模块开关量
    // g_vision_switch_pub = nh.advertise<std_msgs::Bool>("/uav" + std::to_string(g_uav_id) + "/prometheus/switch/landpad_det", 10);

    //【发布】发送给控制模块命令
    // g_command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/uav" + std::to_string(g_uav_id) + "/prometheus/command", 10);
    g_command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

    // 自定义发布的画图信息
    plot_pub = nh.advertise<visual_servo_landing::MyPlot>("/MyPlot", 10);
}

// 获取图像特征值
Eigen::Vector3f getImageFeature(Eigen::Matrix3f camera_matrix) {
    // 要返回的特征值 s
    Eigen::Vector3f s = Eigen::Vector3f::Zero();

    // -------------获取图像像素特征点坐标----------------
    Eigen::Matrix<float, 3, 4> pixel_AB;
    Eigen::Matrix<float, 3, 4> pixel_ab;
    Eigen::Matrix<float, 3, 4> Pixel;
    if (exec_state == TRACKING_STAGE1) {
        for (int i = 0; i < 4; i++) pixel_AB(2, i) = 1.0f;
        pixel_AB(0,0) = g_landpad_det.Detection_info.outter_corner0[0];
        pixel_AB(0,1) = g_landpad_det.Detection_info.outter_corner1[0];
        pixel_AB(0,2) = g_landpad_det.Detection_info.outter_corner2[0];
        pixel_AB(0,3) = g_landpad_det.Detection_info.outter_corner3[0];
        pixel_AB(1,0) = g_landpad_det.Detection_info.outter_corner0[1];
        pixel_AB(1,1) = g_landpad_det.Detection_info.outter_corner1[1];
        pixel_AB(1,2) = g_landpad_det.Detection_info.outter_corner2[1];
        pixel_AB(1,3) = g_landpad_det.Detection_info.outter_corner3[1];

        Pixel = pixel_AB;
    }
    else if (exec_state == TRACKING_STAGE2) {
        for (int i = 0; i < 4; i++) pixel_ab(2, i) = 1.0f;
        pixel_ab(0,0) = g_landpad_det.Detection_info.inner_corner0[0];
        pixel_ab(0,1) = g_landpad_det.Detection_info.inner_corner1[0];
        pixel_ab(0,2) = g_landpad_det.Detection_info.inner_corner2[0];
        pixel_ab(0,3) = g_landpad_det.Detection_info.inner_corner3[0];
        pixel_ab(1,0) = g_landpad_det.Detection_info.inner_corner0[1];
        pixel_ab(1,1) = g_landpad_det.Detection_info.inner_corner1[1];
        pixel_ab(1,2) = g_landpad_det.Detection_info.inner_corner2[1];
        pixel_ab(1,3) = g_landpad_det.Detection_info.inner_corner3[1];

        Pixel = pixel_ab;
    }

    // >>>>>>>>>>>>>>>>>>>>获取图像特征值s<<<<<<<<<<<<<<<<<<<

    Eigen::Matrix<float, 3, 4> h_Pc;
    h_Pc = camera_matrix.inverse() * Pixel;

    Eigen::RowVector3f row3rd = R_c2v.row(2);
    float zeta[4];
    zeta[0] = 1.0/(row3rd*h_Pc.col(0));
    zeta[1] = 1.0/(row3rd*h_Pc.col(1));
    zeta[2] = 1.0/(row3rd*h_Pc.col(2));
    zeta[3] = 1.0/(row3rd*h_Pc.col(3));

    Eigen::Matrix<float, 3, 4> h_Pv;
    h_Pv.col(0) = zeta[0]*R_c2v*h_Pc.col(0);
    h_Pv.col(1) = zeta[1]*R_c2v*h_Pc.col(1);
    h_Pv.col(2) = zeta[2]*R_c2v*h_Pc.col(2);
    h_Pv.col(3) = zeta[3]*R_c2v*h_Pc.col(3);

    float xg = h_Pv.row(0).sum()/4.0;
    float yg = h_Pv.row(1).sum()/4.0;
    float a = 0.0;
    for(int i=0; i<4; i++)
    {
        a = a + pow(h_Pv(0,i)-xg, 2) + pow(h_Pv(1,i)-yg, 2);
    }

    s[2] = sqrt(ad/a);
    s[0] = s[2]*xg;
    s[1] = s[2]*yg;

    return s;
}


void mainControl(ros::Time start_time, ros::Time last_time) {
    // 记录当前时间
    ros::Time current_time = ros::Time::now();

    // 计算时间间隔，elapsed_time是运行周期，t是时间轴
    ros::Duration elapsed_time = current_time - last_time;
    ros::Duration t = current_time - start_time;
    ss << "t = " << t.toSec() << std::endl;

    // 第一阶段降落满足时间
    ros::Time stable_start;
    ros::Duration stable_duration(10,0);

    // >>>>>>>>>>>>>>>>>>> 计算期望速度 v_r <<<<<<<<<<<<<<<<<<<<<<
    rho_s(0,0) = (rho_st0(0) - rho_stinf(0))*std::exp(-ls(0)*t.toSec()) + rho_stinf(0);
    rho_s(1,1) = (rho_st0(1) - rho_stinf(1))*std::exp(-ls(1)*t.toSec()) + rho_stinf(1);
    rho_s(2,2) = (rho_st0(2) - rho_stinf(2))*std::exp(-ls(2)*t.toSec()) + rho_stinf(2);
    // std::cout << "rho_s = " << std::endl << rho_s << std::endl;

    d_rho_s(0,0) = -ls(0)*(rho_st0(0) - rho_stinf(0))*std::exp(-ls(0)*t.toSec());
    d_rho_s(1,1) = -ls(1)*(rho_st0(1) - rho_stinf(1))*std::exp(-ls(1)*t.toSec());
    d_rho_s(2,2) = -ls(2)*(rho_st0(2) - rho_stinf(2))*std::exp(-ls(2)*t.toSec());
    // std::cout << "d_rho_s = " << std::endl << d_rho_s << std::endl;


    Eigen::Vector3f s = getImageFeature(camera_matrix);

    e_s = s - sd;
    xi_s = rho_s.inverse()*e_s;

    Eigen::Vector3f abs_value = e_s.cwiseAbs();
    // ss << "abs_value = " << endl << abs_value << endl;

    // 当e_s所有误差小于0.1的时间大于三秒，切换到下一阶段降落
    // if ((abs_value.array() < 0.2).all() && exec_state == TRACKING_STAGE1) {
    //     if (!tracking_stage_change_flag) {
    //         stable_start = ros::Time::now();
    //         tracking_stage_change_flag = true;
    //     }
    //     // 看持续时间如果大于三秒，则进入第二阶段
    //     else {
    //         if ((ros::Time::now() - stable_start).toSec() > stable_duration.toSec()) {
    //             exec_state = TRACKING_STAGE2;
    //         }
    //     }
    // }
    // // 否则就不需要切换状态
    // else {
    //     tracking_stage_change_flag = false;
    // }

    ss << "exec_state = " << exec_state << std::endl;
    
    ss << RED << "s = " << std::endl << TAIL << s << std::endl;
    ss << RED << "e_s = " << std::endl << TAIL << e_s << std::endl;
    // std::cout << "xi_s = (< 1)" << std::endl << xi_s << std::endl;

    ep_s(0) = 0.5*log((1+xi_s(0))/(1-xi_s(0)));
    ep_s(1) = 0.5*log((1+xi_s(1))/(1-xi_s(1)));
    ep_s(2) = 0.5*log((1+xi_s(2))/(1-xi_s(2)));
    // std::cout << "ep_s = " << std::endl << ep_s << std::endl;

    D_s(0,0) = 1/(1-pow(xi_s(0),2));
    D_s(1,1) = 1/(1-pow(xi_s(1),2));
    D_s(2,2) = 1/(1-pow(xi_s(2),2));
    // std::cout << "D_s = " << std::endl << D_s << std::endl;

    L_s = rho_s.inverse()*D_s;
    // std::cout << "L_s = " << std::endl << L_s << std::endl;

    hat_vm += (Ups.inverse()*(-K_m*hat_vm + L_s.transpose()*ep_s/Zd))*elapsed_time.toSec();
    // std::cout << "hat_vm = " << std::endl << hat_vm << std::endl;

    v_r = Zd*(K_s*L_s.inverse()*ep_s - d_rho_s*xi_s) + hat_vm;
    ss << RED << "v_r = " << std::endl << TAIL <<  v_r << std::endl;

    // >>>>>>>>>>>>>>>>>>>>>>惯性系速度控制<<<<<<<<<<<<<<<<<<<<<<<<<<
    g_command_now.Mode = prometheus_msgs::ControlCommand::Move;
    // g_command_now.Reference_State.Move_frame = prometheus_msgs::PositionReference::BODY_FRAME;
    g_command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;

    // 使用机体惯性系作为误差进行惯性系的速度控制
    g_command_now.Reference_State.velocity_ref[0] = v_r[0];
    g_command_now.Reference_State.velocity_ref[1] = v_r[1]; 
    g_command_now.Reference_State.velocity_ref[2] = v_r[2];
    g_command_now.Reference_State.Yaw_Rate_Mode = true;
    g_command_now.Reference_State.yaw_rate_ref = 0;
    // 移动过程中，调节航向角
    // g_command_now.yaw_ref = 3.14*1/12*ros::Time::now().toSec();

    // 输出变量，作图
    for(int i = 0; i < 3; i++)
    {
        plot_msg.position[i] = g_UAVState.position[i];
        plot_msg.rho_s[i] = rho_s(i,i);
        plot_msg.s_ohr[i] = -rho_s(i,i);
        plot_msg.s[i] = s[i];
        plot_msg.e_s[i] = e_s[i];
        plot_msg.hat_vm[i] = hat_vm(i);
        plot_msg.v_r[i] = v_r(i);
    }

}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_landing");
    ros::NodeHandle nh("~");

    // 节点运行频率： 30hz
    ros::Rate rate(30.0);

    // 记录时间
    ros::Time stage1_start_time;
    ros::Time stage2_start_time;
    ros::Time last_time;

    // 相机内参
    camera_matrix(0, 0) = fx;
    camera_matrix(0, 2) = cx;
    camera_matrix(1, 1) = fy;
    camera_matrix(1, 2) = cy;
    camera_matrix(2, 2) = 1.0f;

    // 读取配置参数
    readParams(nh);
    // 订阅话题
    topicSub(nh);
    // 发布话题
    topicAdv(nh);

    g_command_now.Command_ID = 0;
    exec_state = EXEC_STATE::WAITING;
    while (ros::ok())
    {
        //回调
        ros::spinOnce();
        
        g_command_now.Command_ID = g_command_now.Command_ID + 1;
        g_command_now.header.stamp = ros::Time::now();
        g_command_now.source = "visual_servo_landing";

        // 等待进入COMMAND_CONTROL模式，且每次进入时重置时间和其他关键变量
        // if (g_uavcontrol_state.control_state != prometheus_msgs::UAVControlState::COMMAND_CONTROL)
        if (curr_mode != "OFFBOARD")
        {
            exec_state = EXEC_STATE::WAITING;
            ss << "current mode: " << RED << curr_mode << endl
                << BLUE << "Waiting for enter OFFBOARD state \n";
            // PCOUT(1, TAIL, "Waiting for enter OFFBOARD state");
            // continue;
        }

        switch (exec_state)
        {
            // 初始状态，等待视觉检测结果
            case WAITING:
            {
                // 检测到landingpad就开启降落TRACKING
                if (g_landpad_det.is_detected && curr_mode == "OFFBOARD")
                {
                    exec_state = TRACKING_STAGE1;
                    // 记录当前时间，重新开始程序
                    stage1_start_time = ros::Time::now();
                    last_time = ros::Time::now();
                    hat_vm.setZero(3);
                    v_r.setZero(3);

                    PCOUT(0, BLUE, "肯定是这里开始了阿.");
                    break;
                }

                // 发送视觉节点启动指令
                // vision_switch.data = true;
                // g_vision_switch_pub.publish(vision_switch);

                // 默认高度为2米，Modules/uav_control/launch/uav_control_outdoor.yaml
                g_command_now.Mode = prometheus_msgs::ControlCommand::Hold;
                ss << GREEN << "Waiting for the detection result. \n";
                // PCOUT(1, GREEN, "Waiting for the detection result.");
                break;
            }
            // 追踪状态
            case TRACKING_STAGE1:
            {
                // 丢失,进入LOST状态
                if (!g_landpad_det.is_detected)
                {
                    exec_state = LOST;
                    PCOUT(0, YELLOW, "Lost the Landing Pad.");
                    break;
                }

                ad = 1e-1;
                mainControl(stage1_start_time, last_time);

                // 更新时间
                last_time = ros::Time::now();
                stage2_start_time = ros::Time::now();

                // PCOUT(0, RED, "运行到这里了吗.");
                break;
            }
            case TRACKING_STAGE2:
            {
                // 丢失,进入LOST状态
                if (!g_landpad_det.is_detected)
                {
                    exec_state = LOST;
                    PCOUT(0, YELLOW, "Lost the Landing Pad.");
                    break;
                }

                ad = 0.5e-1;
                mainControl(stage2_start_time, last_time);

                // 更新时间
                last_time = ros::Time::now();

                // PCOUT(0, RED, "运行到这里了吗.");
                break;
            }
            // 目标丢失常识自动找回，在丢失目标后无人机先原定悬停一段时间，如果悬停到一段时候后
            // 仍然没有找到目标，则无人机持续向上飞行，到达一定高度后仍然未发现目标，判定自动
            // 降落任务失败，原点进入降落模式。
            case LOST:
            {
                static int lost_time = 0;
                lost_time++;

                PCOUT(0, YELLOW, "MOTHERFOULLER.");

                // 首先是悬停等待 尝试得到图像, 如果仍然获得不到图像 则原地上升
                if (lost_time < 10.0)
                {
                    g_command_now.Mode = prometheus_msgs::ControlCommand::Hold;

                    ros::Duration(0.4).sleep();
                }
                else
                {
                    g_command_now.Mode = prometheus_msgs::ControlCommand::Move;
                    g_command_now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;
                    g_command_now.Reference_State.velocity_ref[0] = 0.0;
                    g_command_now.Reference_State.velocity_ref[1] = 0.0;
                    g_command_now.Reference_State.velocity_ref[2] = 0.1;
                    g_command_now.Reference_State.Yaw_Rate_Mode = true;
                    g_command_now.Reference_State.yaw_rate_ref = 0;

                    // 如果上升超过原始高度，则认为任务失败，则直接降落
                    if (g_UAVState.position[2] >= max_height)
                    {
                        exec_state = LANDING;
                        lost_time = 0;
                        PCOUT(0, RED, "Mission failed, landing... ");
                    }
                }

                // 重新获得信息,进入TRACKING
                if (g_landpad_det.is_detected)
                {
                    exec_state = TRACKING_STAGE1;
                    PCOUT(0, GREEN, "Regain the Landing Pad.");
                }
                break;
            }
            case LANDING:
            {
                g_command_now.Mode = prometheus_msgs::ControlCommand::Land;
                break;
            }
        }

        // >>>>>>>>>>>>>>>>>Publish & Print<<<<<<<<<<<<<<<<<<<
        g_command_pub.publish(g_command_now);
        plot_pub.publish(plot_msg);

        PCOUT(0.2, TAIL, ss.str());
        ss.str("");
        rate.sleep();
    }

    return 0;
}